/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright
 * 	  notice, this list of conditions and the following disclaimer.
 * 	* Redistributions in binary form must reproduce the above copyright
 *	  notice, this list of conditions and the following disclaimer in the
 * 	  documentation and/or other materials provided with the distribution.
 * 	* Neither the name of the University of California, Berkeley nor the
 * 	  names of its contributors may be used to endorse or promote products
 * 	  derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * @file peripheral.cpp
 * @author haibo
 * @date Sep 1, 2022
 * @brief Example code to control peripheral (jecking, rotary, forklift, roller, etc)
 * Contact: haibo.yang@lgmgim.cn
 */

#include <agv_peripheral_controller/amr/peripheral.h>
#include <agv_peripheral_controller/toolKit.h>
#include <shared_parameters/shared_params_interface.hpp>

using namespace std;
#define PICK_NAME_CONFIG "pick_name"
#define DROP_NAME_CONFIG "drop_name"
#define ADJUST_NAME_CONFIG "adjust_name"

#define ORDER_UP_OLD_VERSION "1"   // 兼容旧版本动作指令动作上升指令
#define ORDER_DOWN_OLD_VERSION "2" // 兼容旧版本动作下降指令
namespace agv_peripheral
{

	AgvPeripheral::AgvPeripheral(const std::string &name)
		: Node(name), name(name), frequency(50.0f), shutdown_req(false)
	// ,dynamic_reconfigure_server(NULL)
	{
		get_shared_parameters();
		memset(&os, 0, sizeof(os));
		initialize_action_server();

		find_joint_id = false;
		joint_id[0] = 0;
		joint_id[1] = 1;

		last_jacking_err = false;
		last_rotary_err = false;
		jacking_recover_emit = false;
		rotary_recover_emit = false;

		manual_jacking_active = false;
		rotary_manual_active_clock = false;
		rotary_manual_active_anticlock = false;

		read_record_result = false;
		last_event_time = this->now();
		last_set_stop_time = this->now();

		read_input_data.reset = false;
		read_input_data.emergency_stop_1 = false;
		read_input_data.emergency_stop_2 = false;

		input_status.upControl = false;
		input_status.downControl = false;

		last_read_input_data.reset = false;
		last_read_input_data.emergency_stop_1 = false;
		last_read_input_data.emergency_stop_2 = false;

		jacking_up_manual = false;
		jacking_down_manual = false;

		// 设置上次的值为无效值
		last_joint_record_pos[0] = -1.0;
		last_joint_record_pos[1] = -1.0;

		rclcpp::Time now = this->now();
		jointState_update_time = now;
		operate_time = now;
		last_landmark_shelf_time = now;
		jacking_move_time = now;
    	rotary_move_time = now;
    	rotary_manual_start_time_clock = now;
	    rotary_manual_start_time_anticlock = now;
	    rotary_manual_stop_time_clock = now;
	    rotary_manual_stop_time_anticlock = now;
	    last_jacking_time = now;
	    last_rotary_time = now;
	    last_landmark_shelf_time = now;
	    last_event_time = now;
	    last_set_stop_time = now;
	}

	AgvPeripheral::~AgvPeripheral()
	{
		m_quit_mutex.unlock();
		m_thread.join();

		delete pallet_controller;
	}

	void AgvPeripheral::clear_os()
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		memset(&os, 0, sizeof(os));
		DEBUG_OUT("[AgvPeripheral] clear os value");
	}

	void AgvPeripheral::set_cur_op(periph_op_enum op)
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		if (op)
		{
			operate_time = this->now();
		}
		os.cur_op = op;
		DEBUG_OUT("[AgvPeripheral] set op %d", os.cur_op);
	}

	int AgvPeripheral::get_cur_op()
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		int op = os.cur_op;
		return op;
	}

	void AgvPeripheral::set_next_op(periph_op_enum op)
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		os.next_op = op;
	}

	int AgvPeripheral::get_next_op()
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		int op = os.next_op;
		return op;
	}

	void AgvPeripheral::set_cur_state(periph_stata_enum state)
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		if (os.cur_state != state)
		{
			os.cur_state = state;
			DEBUG_OUT("[AgvPeripheral] set cur_state %d", os.cur_state);
		}
	}

	int AgvPeripheral::get_cur_state()
	{
		std::lock_guard<std::mutex> lock(m_mutex);
		int state = os.cur_state;
		return state;
	}

	/** @brief Callback function for the Joint State subscriber */
	void AgvPeripheral::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg)
	{
		joint_state = *msg;
		if (!find_joint_id)
		{
			bool has_jacking = (peripheral_type == JACKING) || (peripheral_type == JACKING_ROTARY);
			bool has_rotary = (peripheral_type == ROTARY) || (peripheral_type == JACKING_ROTARY);
			bool find_jacking = false;
			bool find_rotary = false;

			for (int i = 0; i < joint_state.name.size(); i++) // find jacking and rotary joint
			{
				if (has_jacking && !find_jacking && joint_state.name[i].find(JACKING_JOINT) != string::npos)
				{
					joint_id[0] = i;
					find_jacking = true;
					DEBUG_OUT("[AgvPeripheral] JACKING ID:%d", joint_id[0]);
				}

				if (has_rotary && !find_rotary && joint_state.name[i].find(ROTARY_JOINT) != string::npos)
				{
					joint_id[1] = i;
					find_rotary = true;
					DEBUG_OUT("[AgvPeripheral] ROTARY ID:%d", joint_id[1]);
				}
			}

			if (has_jacking && !find_jacking || has_rotary && !find_rotary)
			{
				DEBUG_ERROR_OUT("[AgvPeripheral] Could not find peripheral jonit number");
			}
			find_joint_id = true;
		}

		if (find_joint_id)
		{
			jointState_update_time = this->now();
			joint_pos[0] = joint_state.position[joint_id[0]];
			joint_pos[1] = joint_state.position[joint_id[1]];
			periphMotor[0].pos = joint_pos[0];
			periphMotor[1].pos = joint_pos[1];
		}
	}

	void AgvPeripheral::check_jointState_out()
	{
		if ((((this->now() - jointState_update_time).seconds() * 1000) > 1000) && jointState_update_time.seconds() > 0) // 1s
		{
			DEBUG_ERROR_OUT("[AgvPeripheral] check joint state timeout");
			set_cur_state(UNCONTROLLED_STATE);
		}
	}

	// void AgvPeripheral::agvInfocb(const agv_msgs::msg::AgvInfo::ConstSharedPtr &msg)
	// {
	// 	u_int64_t state = msg->agv_info;
	// 	input_status.manualState = (state & 1 << 4);
	// 	input_status.handle_emergency_stop = (state & 1 << 15);
	// 	input_status.emgButtonState = (state & 1 << 16);
	// 	input_status.front_and_rear_bumper = (state & 1 << 17);
	// 	if (input_status.manualState)
	// 		pallet_controller->set_manual_mode(true);
	// 	else
	// 		pallet_controller->set_manual_mode(false);
	// }

	void AgvPeripheral::agvModecb(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg)
	{
		uint8_t agv_mode = msg->mode;
		input_status.manualState = agv_mode == agv_msgs::msg::AGVMode::MANUAL;
		pallet_controller->set_manual_mode(input_status.manualState);
	}

	void AgvPeripheral::landmarkShelfCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &data)
	{
		last_landmark_shelf_time = this->now();
		loadInfo.loadid = to_string((int)trunc(data->pose.position.z));
	}

	void AgvPeripheral::driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &data)
	{
		driversInfo = *data;
		bool has_jacking = (peripheral_type == JACKING) || (peripheral_type == JACKING_ROTARY);
		bool has_rotary = (peripheral_type == ROTARY) || (peripheral_type == JACKING_ROTARY);

		uint16_t statusWord = 0x0;

		for (int i = 0; i < driversInfo.drivers.size(); i++)
		{
			statusWord = driversInfo.drivers[i].statusword;

			if (has_jacking && driversInfo.drivers[i].jointname.find(JACKING_JOINT) != string::npos)
			{
				motorStatus[0].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[0].homeState = (statusWord & 0x8000);
				motorStatus[0].motorErr = (statusWord & 0x8);
				if (motorStatus[0].motorErr == 0)
				{
					jacking_recover_emit = false;
				}
				else if (motorStatus[0].motorErr != last_jacking_err && !last_jacking_err)
				{
					jacking_recover_emit = true;
					err_begin_time = this->now();
					DEBUG_OUT("[AgvPeripheral] recover emit. jacking errcode[%d]", motorStatus[0].motorErr);
				}
				motorStatus[0].movingState = (statusWord & 0x400);
				motorStatus[0].ready = driversInfo.drivers[i].ready;
				periphMotor[0].enablestatus = motorStatus[0].enableState;
				periphMotor[0].homefinished = motorStatus[0].homeState;
				periphMotor[0].errstatus = motorStatus[0].motorErr;
				periphMotor[0].ready = motorStatus[0].ready;
				if (statusWord != last_jacking_status_word)
				{
					DEBUG_OUT("[AgvPeripheral] jacking motor statusWord: %x", statusWord);
					DEBUG_OUT("[AgvPeripheral] jacking statusWord[%x] enable[%d] home[%d]", statusWord, motorStatus[0].enableState, motorStatus[0].homeState);
				}
				last_jacking_status_word = statusWord;
				last_jacking_err = (motorStatus[0].motorErr == 0x0);
			}

			if (has_rotary && driversInfo.drivers[i].jointname.find(ROTARY_JOINT) != string::npos)
			{
				motorStatus[1].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[1].homeState = (statusWord & 0x8000);
				motorStatus[1].motorErr = (statusWord & 0x8);
				if (motorStatus[1].motorErr == 0)
				{
					rotary_recover_emit = false;
				}
				else if (motorStatus[1].motorErr != last_rotary_err && !last_rotary_err)
				{
					rotary_recover_emit = true;
					err_begin_time = this->now();
					DEBUG_OUT("[AgvPeripheral] recover emit. rotary errcode[%d]", motorStatus[1].motorErr);
				}
				motorStatus[1].movingState = (statusWord & 0x400);
				motorStatus[1].ready = driversInfo.drivers[i].ready;
				periphMotor[1].enablestatus = motorStatus[1].enableState;
				periphMotor[1].homefinished = motorStatus[1].homeState;
				periphMotor[1].errstatus = motorStatus[1].motorErr;
				periphMotor[1].ready = motorStatus[1].ready;
				if (statusWord != last_rotary_status_word)
				{
					DEBUG_OUT("[AgvPeripheral] rotary motor statusWord: %x", statusWord);
					DEBUG_OUT("[AgvPeripheral] rotary  statusWord[%x] enable[%d] home[%d]", statusWord, motorStatus[1].enableState, motorStatus[1].homeState);
				}
				last_rotary_status_word = statusWord;
				last_rotary_err = (motorStatus[1].motorErr == 0x0);
			}
		}
	}

	void AgvPeripheral::autoStatusCallback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr &msg)
	{
		if (input_status.manualState)
		{
			pallet_controller->set_followup(0);
		}
		else
		{
			pallet_controller->set_followup(msg->rackfollowup);

			if (msg->fromnode != 65535 && msg->tonode != 65535)
			{
				uint8_t forbidRotary = (msg->bits >> bit_forbid_rotary) & 0x1;
				pallet_controller->set_forbid_rotary(forbidRotary);
			}

			uint8_t enablePalletRotate = (msg->bits >> bit_pos_rotate) & 0x1;

			if (enablePalletRotate && !msg->rackfollowup && periphStatus.loaded)
			{
				double target = 0.0;
				if (msg->word1 != -1)
				{
					target = (double)msg->word1 / 180.0 * M_PI * -1;
					pallet_controller->set_rotary_target_position(target);
				}
				else if (msg->pathword1 != -1)
				{
					target = (double)msg->pathword1 / 180.0 * M_PI * -1;
					pallet_controller->set_rotary_target_position(target);
				}
			}
		}
	}

	void AgvPeripheral::readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg)
	{
		std::vector<agv_msgs::msg::PairTypeInt> input_msg = msg->array;

		for (const auto &data : input_msg)
		{
			if (data.key.compare("resetButton") == 0)
			{
				read_input_data.reset = (data.value == 1);
			}
			else if (data.key.compare("emergencyButton") == 0)
			{
				read_input_data.emergency_stop_1 = (data.value == 1);
			}
			else if (data.key.compare("emergencyButton2") == 0)
			{
				read_input_data.emergency_stop_2 = (data.value == 1);
			}

			if (data.key == "upControl")
			{
				// 以上升沿为判断
				if (data.value == 1 && input_status.upControl == 0)
				{
					jacking_up_manual = true;
				}
				else if (data.value == 0)
				{
					jacking_up_manual = false;
				}

				input_status.upControl = data.value;
			}

			if (data.key == "downControl")
			{
				if (data.value == 1 && input_status.downControl == 0)
				{
					jacking_down_manual = true;
				}
				else if (data.value == 0)
				{
					jacking_down_manual = false;
				}
				input_status.downControl = data.value;
			}
		}
	}

	void AgvPeripheral::periphTeleopCtlcb(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg)
	{
		rclcpp::Time now = this->now();
		uint8_t order = msg->order;
		float freq = msg->freq;
		double speed_limit = 0.0;
		// DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "[AgvPeripheral] teleop called. order[%d] freq[%.4f]  current_state[%d]", order, freq, get_cur_state());

		DEBUG_OUT("[TELEOPCB] state: %d  order: %d  clock_active: %d  anticlock_active: %d",
				 get_cur_state(), order, rotary_manual_active_clock, rotary_manual_active_anticlock);

		if ((get_cur_state() != MANUAL_MODE_STATE) || (!freq))
		{
			return;
		}

		if (order == UP)
		{
			manual_jacking_active = true;
			jacking_timeout_time = 5 * (1 / freq);
			manual_target_pos[0] = max_pos[0];
			pallet_controller->set_jacking_target_position(manual_target_pos[0]);
			last_jacking_time = now;
		}
		else if (order == DOWN)
		{
			manual_jacking_active = true;
			jacking_timeout_time = 5 * (1 / freq);
			manual_target_pos[0] = min_pos[0];
			pallet_controller->set_jacking_target_position(manual_target_pos[0]);
			last_jacking_time = now;
		}
		else if (order == CLOCKWISE_ROTATION)
		{
			if (rotary_manual_active_clock == false)
			{
				rotary_manual_start_time_clock = now;
				DEBUG_OUT("[TELEOPCB] set clock rotary start time: %f", rotary_manual_start_time_clock.seconds());
			}
			rotary_manual_active_clock = true;
			rotary_timeout_time = 5 * (1 / freq);
			speed_limit = (now - rotary_manual_start_time_clock).seconds() * rotary_manual_acc * -1;
			manual_target_pos[1] = speed_limit < (-1 * rotary_manual_vel) ? (-1 * rotary_manual_vel) : speed_limit;
			pallet_controller->set_rotary_target_velocity(manual_target_pos[1]);
			last_rotary_time = now;
		}
		else if (order == ANTICLOCKWISE_ROTATION)
		{
			if (rotary_manual_active_anticlock == false)
			{
				rotary_manual_start_time_anticlock = now;
				DEBUG_OUT("[TELEOPCB] set anticlock rotary start time: %f", rotary_manual_start_time_anticlock.seconds());
			}
			rotary_manual_active_anticlock = true;
			rotary_timeout_time = 5 * (1 / freq);
			speed_limit = (now - rotary_manual_start_time_anticlock).seconds() * rotary_manual_acc;
			manual_target_pos[1] = speed_limit > rotary_manual_vel ? rotary_manual_vel : speed_limit;
			pallet_controller->set_rotary_target_velocity(manual_target_pos[1]);
			last_rotary_time = now;
		}
	}

	void AgvPeripheral::set_stop_bits()
	{
		int pallet_state = pallet_controller->get_state();
		if (pallet_state == PalletController::TYPE::TASK || pallet_state == PalletController::TYPE::RESET ||
			pallet_state == PalletController::TYPE::FIX)
		{
			setStop.stop = 1;
			if ((this->now() - last_set_stop_time).seconds() > 0.02)
			{
				set_stop_pub->publish(setStop);
				last_set_stop_time = this->now();
			}
		}
		else
		{
			setStop.stop = 0;
			if ((this->now() - last_set_stop_time).seconds() > 0.1)
			{
				set_stop_pub->publish(setStop);
				last_set_stop_time = this->now();
			}
		}
	}

	/*
	void AgvPeripheral::reconfigCB(agv_peripheral_controller::periphDynParamConfig &config, uint32_t unused_level)
	{
		DEBUG_OUT("Reconfigure request:%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %d", config.JackingMinPos, config.JackingMaxPos, config.RotaryAngularControlKP,
				 config.RotaryAngularControlKI, config.RotaryAngularControlKD, config.RotaryAngularControlCoeff, config.RotaryAngularControlAcc,
				 config.LoadWithQRCode);
		// min_pos[0] = (int)(config.JackingMinPos / reducationRatio[0] * pluses_of_single_circle[0]);
		// max_pos[0] = (int)(config.JackingMaxPos / reducationRatio[0] * pluses_of_single_circle[0]);
		min_pos[0] = config.JackingMinPos;
		max_pos[0] = config.JackingMaxPos;
		if (min_pos[0] < 0.00)
			min_pos[0] = 0.00;
		if (max_pos[0] > 0.05)
			max_pos[0] = 0.05;

		/
		setobject.request.node = JACKING_JOINT;
		setobject.request.object = string(PROVELOBJ);
		setobject.request.value = to_string((int)(config.JackingProfileSpeed / reducationRatio[0] * 60 * 512 * pluses_of_single_circle[0] / 1875));
		setobject.request.cached = false;
		PeripheralObjCtl.call(setobject);
		// min_pos[1] = (int)(config.RotaryMinPos / reducationRatio[1] * pluses_of_single_circle[1]);
		// max_pos[1] = (int)(config.RotaryMaxPos / reducationRatio[1] * pluses_of_single_circle[1]);
		min_pos[1] = config.RotaryMinPos;
		max_pos[1] = config.RotaryMaxPos;

		setobject.request.node = ROTARY_JOINT;
		setobject.request.object = string(PROVELOBJ);
		setobject.request.value = to_string((int)(config.RotaryProfileSpeed / reducationRatio[1] * 60 * 512 * pluses_of_single_circle[1] / 1875));
		setobject.request.cached = false;
		PeripheralObjCtl.call(setobject);
		DEBUG_OUT("Reconfigure response:%.4f, %.4f, %.4f, %.4f", min_pos[0], max_pos[0], max_pos[1], min_pos[1]);
		/
	}
	*/

	bool AgvPeripheral::init()
	{
		int type;
		pallet_controller = new PalletController(shared_from_this());
		pevent.init(shared_from_this());
		get_parameter_or("PeripheralType", type, 2);
		get_parameter_or("LoadWithQRCode", load_with_qrcode, false);
		get_parameter_or("BitPosRotate", bit_pos_rotate, 8);
		get_parameter_or("BitForbidRotary", bit_forbid_rotary, 9);
		get_parameter_or("RotaryManualAcc", rotary_manual_acc, 0.5);
		get_parameter_or("RotaryManualVel", rotary_manual_vel, 0.5);
		get_parameter_or("RotaryReturnRecord", rotary_return_record_pos, false);
		jacking_home_need = SharedParamsInterface::getParam<int>("base", "jacking_joint/homeNeed", false);
		rotary_home_need = SharedParamsInterface::getParam<int>("base", "rotary_joint/homeNeed", false);
		
		periphMotor.resize(2);
		if (type == JACKING)
		{
			DEBUG_OUT("JACKING");
			peripheral_type = JACKING;
			get_parameter_or<float>("JackingMaxPos", max_pos_user[0], 0.05);
			get_parameter_or<float>("JackingMinPos", min_pos_user[0], 0.00);
			DEBUG_OUT("JackingMaxPos: %.4f  JackingMinPos: %.4f", max_pos_user[0], min_pos_user[0]);
			max_pos[0] = max_pos_user[0];
			min_pos[0] = min_pos_user[0];
			periphMotor[0].motor_name = JACKING_JOINT;
		}
		else if (type == ROTARY)
		{
			get_parameter_or<float>("RotaryMaxPos", max_pos_user[1], 90.0);
			get_parameter_or<float>("RotaryMinxPos", min_pos_user[1], -90.0);
			max_pos[1] = max_pos_user[1];
			min_pos[1] = min_pos_user[1];
			periphMotor[1].motor_name = ROTARY_JOINT;
		}
		else if (type == JACKING_ROTARY)
		{
			DEBUG_OUT("JACKING_ROTARY");
			peripheral_type = JACKING_ROTARY;
			get_parameter_or<float>("JackingMaxPos", max_pos_user[0], 0.05);
			get_parameter_or<float>("JackingMinPos", min_pos_user[0], 0.00);
			DEBUG_OUT("JackingMaxPos: %.4f  JackingMinPos: %.4f", max_pos_user[0], min_pos_user[0]);
			get_parameter_or<float>("RotaryMaxPos", max_pos_user[1], 90.0);
			get_parameter_or<float>("RotaryMinxPos", min_pos_user[1], -90.0);
			DEBUG_OUT("RotaryMaxPos: %.4f  RotaryMinPos: %.4f", max_pos_user[1], min_pos_user[1]);
			max_pos[0] = max_pos_user[0];
			min_pos[0] = min_pos_user[0];
			max_pos[1] = max_pos_user[1];
			min_pos[1] = min_pos_user[1];
			periphMotor[0].motor_name = JACKING_JOINT;
			periphMotor[1].motor_name = ROTARY_JOINT;
		}

		// subscribe joint state
		js_sub = this->create_subscription<sensor_msgs::msg::JointState>(
            "/joint_states", 1, std::bind(&AgvPeripheral::jointStateCallback, this, std::placeholders::_1));
        periph_status_pub = this->create_publisher<agv_msgs::msg::PeriphStatus>(
            "/periphStatus", 1);
        periph_teleop_sub = this->create_subscription<agv_msgs::msg::PeriphTeleop>(
            "/base/periphTeleopInfo", 1, std::bind(&AgvPeripheral::periphTeleopCtlcb, this, std::placeholders::_1));
        agvMode_sub = this->create_subscription<agv_msgs::msg::AGVMode>(
            "agvmode", 1, std::bind(&AgvPeripheral::agvModecb, this, std::placeholders::_1));
        PeripheralEnableCtl = this->create_client<agv_srvs::srv::ControllerEnable>(
            "/controller_enable");
        landmark_shelf_sub = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/landmark_shelf", 1, std::bind(&AgvPeripheral::landmarkShelfCallback, this, std::placeholders::_1));
        driver_info_sub = this->create_subscription<agv_msgs::msg::DriversInfo>(
            "/driversInfo", 1, std::bind(&AgvPeripheral::driversInfoCallback, this, std::placeholders::_1));
        set_stop_pub = this->create_publisher<agv_msgs::msg::SetStopBits>(
            "/set_stop_bits", 10);
        auto_status_sub = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
            "/auto_run_status", 10, std::bind(&AgvPeripheral::autoStatusCallback, this, std::placeholders::_1));
        readinput_sub = this->create_subscription<agv_msgs::msg::ReadInPutsNew>(
            "/readInputNew", 10, std::bind(&AgvPeripheral::readInputCallback, this, std::placeholders::_1));
        outputClient = this->create_client<agv_srvs::srv::OutputsNew>(
            "/outputNew");

		// pos_record_file = ros::package::getPath(PACKAGE_NAME) + FILE_NAME;
		pos_record_file = std::string(getenv("HOME")) + FILE_NAME;
		DEBUG_OUT("pos record file path is: %s", pos_record_file.c_str());

		// 确认是否有举升电源继电器
		if (hasliftrelay)
		{
			// 打开举升继电器
			rclcpp::sleep_for(1s);
		    auto request = std::make_shared<agv_srvs::srv::OutputsNew::Request>();
		    agv_msgs::msg::PairTypeInt singleIo;
		    singleIo.key = "liftRelay";
		    singleIo.value = 1;
		    request->array.push_back(singleIo);
		    auto future_result = outputClient->async_send_request(request);
		  
		    if (rclcpp::spin_until_future_complete(
		        this->get_node_base_interface(), 
		        future_result) == rclcpp::FutureReturnCode::SUCCESS)
		    {
		        auto response = future_result.get();
		        if (response->state) {
		            DEBUG_OUT("lift relay is open");
		        } else {
		            DEBUG_OUT("Service returned error state");
		        }
		    } else {
		        DEBUG_ERROR_OUT("Failed to call /outputNew service");
		    }
		}
		
		read_record_result = periphRecordPosRead();
		m_quit_mutex.lock();
		m_thread = std::thread(thread_fun, this);
		return true;
	}

	bool AgvPeripheral::periphReadyCheck()
	{
		bool periphReady = false;
		if (peripheral_type == JACKING)
		{
			periphReady = motorStatus[0].ready;
		}
		else if (peripheral_type == ROTARY)
		{
			periphReady = motorStatus[1].ready;
		}
		else if (peripheral_type == JACKING_ROTARY)
		{
			periphReady = (motorStatus[0].ready) && (motorStatus[1].ready);
		}

		if (!periphReady)
			DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "peripheral motor ready check: jacking[%d] rotary[%d]", motorStatus[0].ready, motorStatus[1].ready);

		return periphReady;
	}

	bool AgvPeripheral::periphMotorEnableCheck()
	{
		bool periphEnabled = false;
		if (peripheral_type == JACKING)
		{
			periphEnabled = motorStatus[0].enableState;
		}
		else if (peripheral_type == ROTARY)
		{
			periphEnabled = motorStatus[1].enableState;
		}
		else if (peripheral_type == JACKING_ROTARY)
		{
			periphEnabled = (motorStatus[0].enableState) && (motorStatus[1].enableState);
		}

		if (!periphEnabled)
			DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "peripheral motor enable check: jacking[%d] rotary[%d]", motorStatus[0].enableState, motorStatus[1].enableState);

		return periphEnabled;
	}

	bool AgvPeripheral::periphMotorErrCheck()
	{
		bool periphinErr = false;
		if (peripheral_type == JACKING)
		{
			periphinErr = motorStatus[0].motorErr;
		}
		else if (peripheral_type == ROTARY)
		{
			periphinErr = motorStatus[1].motorErr;
		}
		else if (peripheral_type == JACKING_ROTARY)
		{
			periphinErr = (motorStatus[0].motorErr) || (motorStatus[1].motorErr);
		}

		if (periphinErr)
			DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "peripheral motor error check: jacking[%d] rotary[%d]", motorStatus[0].motorErr, motorStatus[1].motorErr);
		return periphinErr;
	}

	bool AgvPeripheral::periphHomingCheck()
	{
		bool periphHomed = false;
		if (peripheral_type == JACKING)
		{
			periphHomed = ((motorStatus[0].homeState) && (abs(joint_pos[0]) < 0.0001)) || !jacking_home_need;
		}
		else if (peripheral_type == ROTARY)
		{
			if (rotary_home_need)
				periphHomed = ((motorStatus[1].homeState) && (abs(joint_pos[1]) < 0.0001)) || !rotary_home_need;
			else
				periphHomed = true;
		}
		else if (peripheral_type == JACKING_ROTARY)
		{
			if ((motorStatus[0].homeState || !jacking_home_need) && (motorStatus[1].homeState || !rotary_home_need))
			{
				periphHomed = true;
			}
		}

		if (!periphHomed)
			DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "peripheral motor homing check: jacking[%d] rotary[%d]", motorStatus[0].homeState, motorStatus[1].homeState);

		return periphHomed;
	}

	// void AgvPeripheral::periphRecoverDetect()
	// {
	// 	bool recover = false;
	// 	if (peripheral_type == JACKING)
	// 		recover = jacking_recover_emit;
	// 	else if (peripheral_type == ROTARY)
	// 		recover = rotary_recover_emit;
	// 	else if (peripheral_type == JACKING_ROTARY)
	// 		recover = jacking_recover_emit || rotary_recover_emit;

	// 	if (recover)
	// 	{
	// 		DEBUG_OUT("[AgvPeripheral] recover detected.");
	// 		if (PeripheralRecoverCtl.call(trig_srv))
	// 		{
	// 			DEBUG_OUT("[AgvPeripheral] call recover service success");
	// 		}
	// 		else
	// 		{
	// 			DEBUG_ERROR_OUT("[AgvPeripheral] call recover service fail");
	// 		}
	// 	}
	// 	else if ((motorStatus[0].motorErr || motorStatus[1].motorErr) && (this->now() - err_begin_time).seconds() > 10.0)
	// 	{
	// 		DEBUG_OUT("[AgvPeripheral] long time motor error detected");
	// 		if (PeripheralRecoverCtl.call(trig_srv))
	// 		{
	// 			DEBUG_OUT("[AgvPeripheral] call recover service success");
	// 		}
	// 		else
	// 		{
	// 			DEBUG_ERROR_OUT("[AgvPeripheral] call recover service fail");
	// 		}

	// 		err_begin_time = this->now();
	// 	}

	// 	jacking_recover_emit = false;
	// 	rotary_recover_emit = false;
	// }

	void AgvPeripheral::periphStatusGather()
	{
		bool clean_event = true;
		rclcpp::Time time_now = this->now();
		periphStatus.header.stamp = time_now;

		if (abs(joint_pos[0] - min_pos[0]) > 0.005)
			periphStatus.loaded = true;
		else
			periphStatus.loaded = false;

		loads.clear();
		periphStatus.loads.clear();

		loadInfo.loadposition = periphStatus.loaded ? "1" : "0";
		loadInfo.loadtype = loadInfo.loadposition;

		if ((time_now - last_landmark_shelf_time).seconds() < 1.0)
		{
		}
		else
		{
			loadInfo.loadid = "";

			if (load_with_qrcode && periphStatus.loaded && !input_status.manualState)
			{
				DEBUG_OUT_THROTTLE(this->get_clock(), 3000, "[AgvPeripheral] Package QRCode Lost !!!  now: %f  last: %f", time_now.seconds(), last_landmark_shelf_time.seconds());
				pevent.updateEvent(agv_msgs::msg::AGVEvent::QRCODE_MISS_ERROR, " Package QRCode Lost!!!");
				last_event_time = time_now;
				clean_event = false;
			}
		}

		if (clean_event && pevent.checkEvent(agv_msgs::msg::AGVEvent::QRCODE_MISS_ERROR)) {
			pevent.removeEvent(agv_msgs::msg::AGVEvent::QRCODE_MISS_ERROR);
		}

		loads.push_back(loadInfo);
		periphStatus.loads = loads;

		periphStatus.periphstateinfo = get_cur_state();
		periphStatus.periphmotor = periphMotor;
		periph_status_pub->publish(periphStatus);
	}

	bool AgvPeripheral::periphGetControllerRunningStatus()
	{
		return true;

		/*
		bool jackingControllerIsRunning = false;
		bool rotaryControllerIsRunning = false;
		controller_manager_msgs::srv::ListControllers srv;
		if (peripheralGetcontrollers.call(srv))
		{
			for (int i = 0; i < srv.response.controller.size(); i++)
			{
				controller_manager_msgs::msg::ControllerState controller = srv.response.controller.at(i);
				if (controller.name == "jacking_controller")
				{
					if (controller.state == "running")
					{
						jackingControllerIsRunning = true;
					}
					else
					{
						DEBUG_OUT("jacking_controller is %s", controller.state.c_str());
					}
				}

				if (controller.name == "rotary_controller")
				{
					if (controller.state == "running")
					{
						rotaryControllerIsRunning = true;
					}
					else
					{
						DEBUG_OUT("rotary_controller is %s", controller.state.c_str());
					}
				}
			}
		}
		if (!jackingControllerIsRunning || !rotaryControllerIsRunning)
			DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "get controller state fail. jacking[%d] rotary[%d]", jackingControllerIsRunning, rotaryControllerIsRunning);

		return (jackingControllerIsRunning & rotaryControllerIsRunning);
		*/
	}

	bool AgvPeripheral::periphMoveToRecordPos()
	{

		if (!read_record_result)
		{
			DEBUG_OUT("cannot read record. move to record cancled");
			return true;
		}

		DEBUG_STREAM_OUT("[AgvPeripheral] start jacking_arrive_recordPos=" << jacking_arrive_recordPos);
		// ROS_INFO_THROTTLE(0.2, "read file jacking position: %f  current jacking position: %f", file_record_pos[0], joint_pos[0]);
		// DEBUG_OUT("read file jacking position: %f  current jacking position: %f", file_record_pos[0], joint_pos[0]);

		if (file_record_pos[0] > max_pos[0] || file_record_pos[0] < min_pos[0])
		{
			DEBUG_OUT("[AgvPeripheral] bad record, quit. record_pos[%.4f] max_pos[%.4f] min_pos[%.4f]", file_record_pos[0], max_pos[0], min_pos[0]);
			return true;
		}

		if ((abs(file_record_pos[0]) <= 0.0001) && (abs(file_record_pos[1]) <= 0.0001))
		{
			DEBUG_OUT("[AgvPeripheral] record home. finished");
			return true;
		}

		if (abs(joint_pos[0] - file_record_pos[0]) <= 0.0001)
		{
			RCLCPP_INFO_ONCE(get_logger(), "[AgvPeripheral] jacking reach record pos. finished");
			jacking_arrive_recordPos = true;
			jacking_move_state = false;
		}

		if (peripheral_type == JACKING_ROTARY && rotary_return_record_pos)
		{
			if (abs(joint_pos[1] - file_record_pos[1]) <= 0.01)
			{
				RCLCPP_INFO_ONCE(get_logger(), "[AgvPeripheral] rotary reach record pos. finished");
				rotary_arrive_recordPos = true;
				rotary_move_state = false;
			}
		}

		if ((!jacking_move_state) && (abs(joint_pos[0] - file_record_pos[0]) > 0.0001))
		{
			DEBUG_OUT("[AgvPeripheral] jacking move to record pos, publish: %f current_pos: %f", file_record_pos[0], joint_pos[0]);
			pallet_controller->set_jacking_target_position(file_record_pos[0]);
			jacking_move_state = true;
			jacking_move_time = this->now();
		}

		if (peripheral_type == JACKING_ROTARY && rotary_return_record_pos)
		{
			if ((!rotary_move_state) && (abs(joint_pos[1] - file_record_pos[1]) > 0.01))
			{
				DEBUG_OUT("[AgvPeripheral] rotary move to record pos, publish: %f current_pos: %f", file_record_pos[1], joint_pos[1]);
				pallet_controller->set_rotary_target_position(file_record_pos[1]);
				rotary_move_state = true;
				rotary_move_time = this->now();
			}
		}

		if (jacking_move_state && ((this->now() - jacking_move_time).seconds() > OPERATE_TIMEOUT_S))
		{
			if (motorStatus[0].movingState && abs(joint_pos[0] - file_record_pos[0]) > 0.0001)
			{
				pallet_controller->set_jacking_target_position(joint_pos[0]);
				DEBUG_ERROR_OUT("jacking move to record pos timeout");
				jacking_move_state = false;
			}
		}

		if (peripheral_type == JACKING_ROTARY && rotary_return_record_pos)
		{
			if (rotary_move_state && ((this->now() - rotary_move_time).seconds() > OPERATE_TIMEOUT_S))
			{
				if ((motorStatus[1].movingState) && (abs(joint_pos[1] - file_record_pos[1]) > 0.01))
				{
					pallet_controller->set_rotary_target_position(joint_pos[1]);
					DEBUG_ERROR_OUT("jacking move to record pos timeout");
					rotary_move_state = false;
				}
			}
		}

		if (peripheral_type == JACKING_ROTARY && rotary_return_record_pos)
		{

			DEBUG_STREAM_OUT("[AgvPeripheral] peripheral_type=" << peripheral_type << ",rotary_return_record_pos=" << rotary_return_record_pos
															   << ",jacking_arrive_recordPos=" << jacking_arrive_recordPos
															   << ",rotary_arrive_recordPos=" << rotary_arrive_recordPos

			);

			return jacking_arrive_recordPos && rotary_arrive_recordPos;
		}

		DEBUG_STREAM_OUT("[AgvPeripheral] end jacking_arrive_recordPos=" << jacking_arrive_recordPos);

		return jacking_arrive_recordPos; //  & rotary_arrive_recordPos;
	}

	void AgvPeripheral::periphStateMachineUpdate()
	{

		if (!periphInitialized)
		{
			if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck())
			{
				if (periphMoveToRecordPos())
				{
					DEBUG_STREAM_OUT("[AgvPeripheral] periphMoveToRecordPos ok periphInitialized  set true");
					periphInitialized = true;
					set_cur_state(IDLE_STATE);
				}
			}
			else
			{
				DEBUG_OUT("[AgvPeripheral] drivers status check fail. call driver recover");
				set_cur_state(UNCONTROLLED_STATE);

				/*
				if (PeripheralRecoverCtl.call(trig_srv)) {
					DEBUG_OUT("[AgvPeripheral] call recover service success");
				} else {
					DEBUG_ERROR_OUT("[AgvPeripheral] call recover service fail");
				}

				rclcpp::Duration(1).sleep();
				*/
			}
		}

		// DEBUG_STREAM_OUT_THROTTLE(0.5,"[AgvPeripheral] middle get_cur_state=" << get_cur_state()<<",periphInitialized="<<periphInitialized);
		if (periphInitialized)
		{
			switch (get_cur_state())
			{
			case IDLE_STATE:
				if (input_status.manualState)
				{
					manual_target_pos[0] = joint_pos[0];
					set_cur_state(MANUAL_MODE_STATE);
				}
				break;
			case MANUAL_MODE_STATE:
				if ((read_input_data.emergency_stop_1 == 1 && last_read_input_data.emergency_stop_1 == 0) ||
					(read_input_data.emergency_stop_2 == 1 && last_read_input_data.emergency_stop_2 == 0))
				{
					std::string jacking_name = std::string("jacking_controller");
					std::string rotary_name = std::string("rotary_controller");
					std::vector<std::string> controllers = {jacking_name, rotary_name};
					
					if (callControllerService(controllers, false) == 0)
					{
						set_cur_op(NO_OP);
						set_next_op(NO_OP);
						set_cur_state(HALT_HANDLE_STATE);
						DEBUG_OUT("[AgvPeripheral] call halt success");
						return;
					}
					else
					{
						DEBUG_ERROR_OUT("[AgvPeripheral] call recover fail");
						return;
					}
				}

				if (!input_status.manualState)
				{
					set_cur_state(IDLE_STATE);
				}
				else
				{
					rclcpp::Time now = this->now();
					if ((now - last_jacking_time).seconds() > jacking_timeout_time && manual_jacking_active)
					{
						DEBUG_OUT("[AgvPeripheral] teleop jacking timeout. now[%.4f] last[%.4f]", now.seconds(), last_jacking_time.seconds());
						pallet_controller->set_jacking_target_position(joint_pos[0]);
						manual_jacking_active = false;
					}

					if ((now - last_rotary_time).seconds() > rotary_timeout_time)
					{
						if (rotary_manual_active_clock)
						{
							rotary_manual_stop_time_clock = now;
							rotary_manual_stop_flag_clock = true;
							rotary_manual_active_clock = false;
							DEBUG_OUT("[AgvPeripheral] teleop clock rotary timeout. now[%.4f] last[%.4f]", now.seconds(), last_rotary_time.seconds());
						}

						if (rotary_manual_active_anticlock)
						{
							rotary_manual_stop_time_anticlock = now;
							rotary_manual_stop_flag_anticlock = true;
							rotary_manual_active_anticlock = false;
							DEBUG_OUT("[AgvPeripheral] teleop anticlock rotary timeout. now[%.4f] last[%.4f]", now.seconds(), last_rotary_time.seconds());
						}

						if (rotary_manual_stop_flag_clock)
						{
							double speed = manual_target_pos[1] + (now - rotary_manual_stop_time_clock).seconds() * rotary_manual_acc;
							DEBUG_OUT_THROTTLE(this->get_clock(), 100, "[CLOCK ROTARY] manual_target: %.4f  dt: %f  acc: %.4f  speed: %.4f",
											  manual_target_pos[1], (now - rotary_manual_stop_time_clock).seconds(), rotary_manual_acc, speed);

							if (speed > 0.0)
								rotary_manual_stop_flag_clock = false;
							speed = speed > 0.0 ? 0.0 : speed;
							DEBUG_OUT_THROTTLE(this->get_clock(), 100, "[CLOCK ROTARY] last_target: %.4f  set speed %.4f", manual_target_pos[1], speed);
							pallet_controller->set_rotary_target_velocity(speed);
						}

						if (rotary_manual_stop_flag_anticlock)
						{
							double speed = manual_target_pos[1] - (now - rotary_manual_stop_time_anticlock).seconds() * rotary_manual_acc;
							DEBUG_OUT_THROTTLE(this->get_clock(), 100, "[ANTICLOCK ROTARY] manual_target: %.4f  dt: %f  acc: %.4f  speed: %.4f",
											  manual_target_pos[1], (now - rotary_manual_stop_time_anticlock).seconds(), rotary_manual_acc, speed);
							if (speed < 0.0)
								rotary_manual_stop_flag_anticlock = false;
							speed = speed < 0.0 ? 0.0 : speed;
							DEBUG_OUT_THROTTLE(this->get_clock(), 100, "[ANTICLOCK ROTARY] last_target: %.4f  set speed %.4f", manual_target_pos[1], speed);
							pallet_controller->set_rotary_target_velocity(speed);
						}
					}
					else
					{
						rotary_manual_stop_flag_clock = false;
						rotary_manual_stop_flag_anticlock = false;
					}

					if (jacking_up_manual)
					{
						pallet_controller->set_jacking_target_position(max_pos[0]);
						last_jacking_time = this->now();
					}
					else if (jacking_down_manual)
					{
						pallet_controller->set_jacking_target_position(min_pos[0]);
						last_jacking_time = this->now();
					}
				}
				break;
			case HALT_HANDLE_STATE:
				// if (!input_status.emgButtonState && !input_status.handle_emergency_stop && !input_status.front_and_rear_bumper)
				if (read_input_data.reset == 1 && last_read_input_data.reset == 0 &&
					read_input_data.emergency_stop_1 == 0 && read_input_data.emergency_stop_2 == 0)
				{
					std::string jacking_name = std::string("jacking_controller");
					std::string rotary_name = std::string("rotary_controller");
					std::vector<std::string> controllers = {jacking_name, rotary_name};
					
					if (callControllerService(controllers, true) == 0)
					{
						DEBUG_OUT("[AgvPeripheral] idle state");
						set_cur_state(IDLE_STATE);
					}
					else
					{
						DEBUG_ERROR_OUT("[AgvPeripheral] call recover fail");
					}
				}
				break;
			case JACKING_OP_STATE_UP:
			case JACKING_OP_STATE_DOWN:
			case JACKING_SUCCESS_STATE:
			case ROTARY_OP_STATE_CLOCKWISH:
			case ROTARY_OP_STATE_ANTICLOCKWISH:
			case ROTARY_OP_STATE_ROTATION:
			case ROTARY_SUCCESS_STATE:
				// if (input_status.emgButtonState || input_status.handle_emergency_stop || input_status.front_and_rear_bumper)
				if ((read_input_data.emergency_stop_1 == 1 && last_read_input_data.emergency_stop_1 == 0) ||
					(read_input_data.emergency_stop_2 == 1 && last_read_input_data.emergency_stop_2 == 0))
				{
					std::string jacking_name = std::string("jacking_controller");
					std::string rotary_name = std::string("rotary_controller");
					std::vector<std::string> controllers = {jacking_name, rotary_name};
					
					if (callControllerService(controllers, false) == 0)
					{
						current_result_->success = FAILED;
						current_result_->message = "Operate halt";
						current_goal_handle_->succeed(current_result_);
						clear_current_action();
						set_cur_op(NO_OP);
						set_next_op(NO_OP);
						set_cur_state(HALT_HANDLE_STATE);
						DEBUG_OUT("[AgvPeripheral] call halt success,operation is Completed");
					}
					else
					{
						DEBUG_ERROR_OUT("[AgvPeripheral] call recover fail");
					}
				}
				break;
			case UNCONTROLLED_STATE:
				if (get_cur_op())
				{
					current_result_->success = FAILED;
					current_result_->message = "Operate fail";
					current_goal_handle_->succeed(current_result_);
					clear_current_action();
					set_cur_op(NO_OP);
					set_next_op(NO_OP);
					DEBUG_OUT("[AgvPeripheral] operation is uncompleted, motor is uncontrollable");
				}

				if (read_input_data.reset == 1 && last_read_input_data.reset == 0 &&
					read_input_data.emergency_stop_1 == 0 && read_input_data.emergency_stop_2 == 0)
				{
					std::string jacking_name = std::string("jacking_controller");
					std::string rotary_name = std::string("rotary_controller");
					std::vector<std::string> controllers = {jacking_name, rotary_name};
					
					if (callControllerService(controllers, true) == 0)
					{
						DEBUG_OUT("[AgvPeripheral] call recover success");
					}
					else
					{
						DEBUG_ERROR_OUT("[AgvPeripheral] call recover fail");
					}
				}

				if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck() && !periphMotorErrCheck())
				{
					set_cur_state(IDLE_STATE);
					DEBUG_OUT("[AgvPeripheral] uncontrolled state recovered, set idle state");
				}

				break;
			default:
				break;
			}

			if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck() && !periphMotorErrCheck())
			{
			}
			else
			{

				if (get_cur_state() != HALT_HANDLE_STATE)
				{
					set_cur_state(UNCONTROLLED_STATE);
					DEBUG_OUT_THROTTLE(this->get_clock(), 1000, "[AgvPeripheral] motor check error, set uncontrolled state");
				}
			}

			last_read_input_data.reset = read_input_data.reset;
			last_read_input_data.emergency_stop_1 = read_input_data.emergency_stop_1;
			last_read_input_data.emergency_stop_2 = read_input_data.emergency_stop_2;
		}
	}

	void AgvPeripheral::spin()
	{
		double period = 1.0 / frequency;
		rclcpp::Rate spin_rate(frequency);

		while (!shutdown_req && rclcpp::ok())
		{
			if (get_cur_op())
			{
				switch (get_cur_op())
				{
				case UP:
					if (abs(joint_pos[0] - max_pos[0]) < 0.0001)
					{
						set_cur_state(JACKING_SUCCESS_STATE);
						if (get_next_op())
						{
							current_feedback_->state = get_cur_state();
							current_feedback_->percent_complete = 1.0f;
							current_feedback_->feedback_action = current_action;
							current_goal_handle_->publish_feedback(current_feedback_);
							set_cur_op((periph_op_enum)get_next_op());
							set_next_op(NO_OP);
							DEBUG_OUT("[AgvPeripheral] Jacking UP Complete and Next Operate:%d", os.next_op);
							executeOP(get_cur_op());
						}
						else
						{
							current_result_->success = SUCCESS;
							current_result_->message = "Operate successed";
							current_result_->result_action = current_action;
							current_goal_handle_->succeed(current_result_);
							clear_current_action();
							// memset(&current_action, 0, sizeof(current_action));
							DEBUG_OUT("[AgvPeripheral] Jacking UP Complete");
							clear_os();
						}
					}
					else
					{
						current_feedback_->state = get_cur_state();
						current_feedback_->percent_complete = abs(joint_pos[0] - previous_pos[0]) * 1.0f / abs(max_pos[0] - previous_pos[0]);
						current_feedback_->feedback_action = current_action;
						DEBUG_OUT("[AgvPeripheral] Jacking Up Complete:%.2f,curent pos is %.4f", current_feedback_->percent_complete, joint_pos[0]);
						current_goal_handle_->publish_feedback(current_feedback_);
					}
					break;

				case DOWN:
					if (abs(joint_pos[0] - min_pos[0]) < 0.0005)
					{
						set_cur_state(JACKING_SUCCESS_STATE);
						current_result_->success = SUCCESS;
						current_result_->message = "Operate successed";
						current_result_->result_action = current_action;
						current_goal_handle_->succeed(current_result_);
						clear_current_action();
						// memset(&current_action, 0, sizeof(current_action));
						DEBUG_OUT("[AgvPeripheral] Jacking Down Complete");
						clear_os();
					}
					else
					{
						current_feedback_->state = get_cur_state();
						current_feedback_->percent_complete = abs(joint_pos[0] - previous_pos[0]) * 1.0f / abs(previous_pos[0] - min_pos[0]);
						current_feedback_->feedback_action = current_action;
						DEBUG_OUT("[AgvPeripheral] Jacking Down Complete:%.2f,curent pos is %.4f", current_feedback_->percent_complete, joint_pos[0]);
						current_goal_handle_->publish_feedback(current_feedback_);
					}
					break;

				case CLOCKWISE_ROTATION:
					if (abs(joint_pos[1] - min_pos[1]) < 0.001)
					{
						set_cur_state(ROTARY_SUCCESS_STATE);
						if (get_next_op())
						{
							current_feedback_->state = get_cur_state();
							current_feedback_->percent_complete = 1.0f;
							current_feedback_->feedback_action = current_action;
							current_goal_handle_->publish_feedback(current_feedback_);
							set_cur_op((periph_op_enum)get_next_op());
							set_next_op(NO_OP);
							DEBUG_OUT("[AgvPeripheral] Rotary Clockwise Rotation Complete and Next Operate:%d", os.next_op);
							executeOP(get_cur_op());
						}
						else
						{
							current_result_->success = SUCCESS;
							current_result_->message = "Operate successed";
							current_result_->result_action = current_action;
							current_goal_handle_->succeed(current_result_);
							clear_current_action();
							// memset(&current_action, 0, sizeof(current_action));
							DEBUG_OUT("[AgvPeripheral] Rotary Clockwise Rotation Complete");
							clear_os();
						}
					}
					else
					{
						current_feedback_->state = get_cur_state();
						current_feedback_->percent_complete = abs(joint_pos[1] - previous_pos[1]) * 1.0f / abs(previous_pos[1] - min_pos[1]);
						current_feedback_->feedback_action = current_action;
						DEBUG_OUT("[AgvPeripheral] Rotary Clockwise Complete:%.2f,curent pos is %.4f", current_feedback_->percent_complete, joint_pos[1]);
						current_goal_handle_->publish_feedback(current_feedback_);
					}
					break;

				case ANTICLOCKWISE_ROTATION:
					if (abs(joint_pos[1] - max_pos[1]) < 0.2)
					{
						set_cur_state(ROTARY_SUCCESS_STATE);
						if (get_next_op())
						{
							current_feedback_->state = get_cur_state();
							current_feedback_->percent_complete = 1.0f;
							current_feedback_->feedback_action = current_action;
							current_goal_handle_->publish_feedback(current_feedback_);
							set_cur_op((periph_op_enum)get_next_op());
							set_next_op(NO_OP);
							DEBUG_OUT("[AgvPeripheral] Rotary ANTIClockwise Rotation Complete and Next Operate:%d", os.next_op);
							executeOP(get_cur_op());
						}
						else
						{
							current_result_->success = SUCCESS;
							current_result_->message = "Operate successed";
							current_result_->result_action = current_action;
							current_goal_handle_->succeed(current_result_);
							clear_current_action();
							// memset(&current_action, 0, sizeof(current_action));
							DEBUG_OUT("[AgvPeripheral] Rotary ANTIClockwise Rotation Complete");
							clear_os();
						}
					}
					else
					{
						current_feedback_->state = get_cur_state();
						current_feedback_->percent_complete = abs(joint_pos[1] - previous_pos[1]) * 1.0f / abs(max_pos[1] - previous_pos[1]);
						current_feedback_->feedback_action = current_action;
						DEBUG_OUT("[AgvPeripheral] Rotary ANTIClockwise Complete:%.2f,curent pos is %.4f", current_feedback_->percent_complete, joint_pos[1]);
						current_goal_handle_->publish_feedback(current_feedback_);
					}
					break;

				case ROTATION:
					if (abs(joint_pos[1] - rotary_target_pos) < 0.001)
					{
						set_cur_state(ROTARY_SUCCESS_STATE);
						if (get_next_op())
						{
							current_feedback_->state = get_cur_state();
							current_feedback_->percent_complete = 1.0f;
							current_feedback_->feedback_action = current_action;
							current_goal_handle_->publish_feedback(current_feedback_);
							set_cur_op((periph_op_enum)get_next_op());
							set_next_op(NO_OP);
							DEBUG_OUT("[AgvPeripheral] Rotary Rotation Complete and Next Operate:%d", os.next_op);
							executeOP(get_cur_op());
						}
						else
						{
							current_result_->success = SUCCESS;
							current_result_->message = "Operate successed";
							current_result_->result_action = current_action;
							current_goal_handle_->succeed(current_result_);
							clear_current_action();
							// memset(&current_action, 0, sizeof(current_action));
							DEBUG_OUT("[AgvPeripheral] Rotary Rotation Complete");
							clear_os();
						}
					}
					else
					{
						current_feedback_->state = get_cur_state();
						current_feedback_->percent_complete = abs(joint_pos[1] - previous_pos[1]) * 1.0f / abs(previous_pos[1] - rotary_target_pos);
						current_feedback_->feedback_action = current_action;
						DEBUG_OUT("[AgvPeripheral] Rotary Complete:%.2f,curent pos is %.4f", current_feedback_->percent_complete, joint_pos[1]);
						current_goal_handle_->publish_feedback(current_feedback_);
					}
					break;

				default:
					break;
				}
			}

			if ((get_cur_op() == UP) || (get_cur_op() == DOWN))
			{
				if ((this->now() - operate_time).seconds() > OPERATE_TIMEOUT_S)
				{
					DEBUG_OUT("[AgvPeripheral] now time is %f,operate_time is %f", this->now().seconds(), operate_time.seconds());
					if ((motorStatus[0].movingState) && (abs(joint_pos[0] - previous_pos[0]) < 0.0001))
					{
						DEBUG_OUT("[AgvPeripheral] movingState is %d,joint_pos is %.4f,previous_pos is %.4f", motorStatus[0].movingState, joint_pos[0], previous_pos[0]);
						pallet_controller->set_jacking_target_position(joint_pos[0]);
						clear_os();
						current_result_->success = FAILED;
						current_result_->message = "Operate timeout";
						current_result_->result_action = current_action;
						current_goal_handle_->succeed(current_result_);
						clear_current_action();
						// memset(&current_action, 0, sizeof(current_action));
						DEBUG_ERROR_OUT("[AgvPeripheral] Jacking operate timeout");
						return;
					}
					else if ((this->now() - operate_time).seconds() > MOVE_TIMEOUT_S)
					{
						clear_os();
						current_result_->success = FAILED;
						current_result_->message = "move timeout";
						current_result_->result_action = current_action;
						current_goal_handle_->succeed(current_result_);
						clear_current_action();
						// memset(&current_action, 0, sizeof(current_action));
						DEBUG_ERROR_OUT("[AgvPeripheral] Jacking move timeout");
						return;
					}
				}
			}
			else if ((get_cur_op() == CLOCKWISE_ROTATION) || (get_cur_op() == ANTICLOCKWISE_ROTATION) || (get_cur_op() == ROTATION))
			{
				if ((this->now() - operate_time).seconds() > OPERATE_TIMEOUT_S)
				{
					if ((motorStatus[1].movingState) && (abs(joint_pos[1] - previous_pos[1]) < 0.0001))
					{
						pallet_controller->set_rotary_target_position(joint_pos[1]);
						clear_os();
						current_result_->success = FAILED;
						current_result_->message = "Operate timeout";
						current_result_->result_action = current_action;
						current_goal_handle_->succeed(current_result_);
						clear_current_action();
						// memset(&current_action, 0, sizeof(current_action));
						DEBUG_ERROR_OUT("[AgvPeripheral] ratory operate timeout");
						return;
					}
					else if ((this->now() - operate_time).seconds() > MOVE_TIMEOUT_S)
					{
						clear_os();
						current_result_->success = FAILED;
						current_result_->message = "move timeout";
						current_result_->result_action = current_action;
						current_goal_handle_->succeed(current_result_);
						clear_current_action();
						// memset(&current_action, 0, sizeof(current_action));
						DEBUG_ERROR_OUT("[AgvPeripheral] Jacking move timeout");
						return;
					}
				}
			}
			periphStateMachineUpdate();

			periphStatusGather();

			pallet_controller->update();
			set_stop_bits();

			spin_rate.sleep();
		}
	}

	bool AgvPeripheral::executeOP(const int order)
	{
		bool success = true;
		switch (order)
		{
		case UP:
			previous_pos[0] = joint_pos[0];
			pallet_controller->set_jacking_target_position(max_pos[0]);
			DEBUG_OUT("[AgvPeripheral] set order up, target pos is %.4f", max_pos[0]);
			set_cur_state(JACKING_OP_STATE_UP);
			break;

		case DOWN:
			previous_pos[0] = joint_pos[0];
			pallet_controller->set_jacking_target_position(min_pos[0]);
			DEBUG_OUT("[AgvPeripheral] set order down, target pos is %.4f", min_pos[0]);
			set_cur_state(JACKING_OP_STATE_DOWN);
			break;

		case CLOCKWISE_ROTATION:
			previous_pos[1] = joint_pos[1];
			pallet_controller->set_rotary_target_position(min_pos[1]);
			DEBUG_OUT("[AgvPeripheral] set order Clockwise, target pos is %.4f", min_pos[1]);
			set_cur_state(ROTARY_OP_STATE_CLOCKWISH);
			break;

		case ANTICLOCKWISE_ROTATION:
			previous_pos[1] = joint_pos[1];
			pallet_controller->set_rotary_target_position(max_pos[1]);
			DEBUG_OUT("[AgvPeripheral] set order Anticlockwise,target pos is %.4f", max_pos[1]);
			set_cur_state(ROTARY_OP_STATE_ANTICLOCKWISH);
			break;

		case ROTATION:
			previous_pos[1] = joint_pos[1];
			pallet_controller->set_rotary_target_position(rotary_target_pos);
			DEBUG_OUT("[AgvPeripheral] set order rotation,target pos is %.4f", rotary_target_pos);
			set_cur_state(ROTARY_OP_STATE_ROTATION);
			break;

		default:
			success = false;
			break;
		}

		return success;
	}

	rclcpp_action::GoalResponse 
	AgvPeripheral::goalCB(
	    const rclcpp_action::GoalUUID & uuid,
	    std::shared_ptr<const PeripheralAction::Goal> goal)
	{
		(void) uuid;
	
		get_parameter_or(PICK_NAME_CONFIG, action_pick, std::string("pick"));
		get_parameter_or(DROP_NAME_CONFIG, action_drop, std::string("drop"));
		get_parameter_or(ADJUST_NAME_CONFIG, action_adjust, std::string("lhdAdjust"));

		if (!goal) {
	        DEBUG_ERROR_OUT("Received null goal pointer");
	        return rclcpp_action::GoalResponse::REJECT;
    	}

    	if (goal->goal_action.actiontype != action_pick && 
	        goal->goal_action.actiontype != action_drop &&
	        goal->goal_action.actiontype != action_adjust &&
	        goal->goal_action.actiontype != ORDER_UP_OLD_VERSION &&
	        goal->goal_action.actiontype != ORDER_DOWN_OLD_VERSION) {
	        DEBUG_WARN_OUT("Invalid action type: %s", goal->goal_action.actiontype.c_str());
	        return rclcpp_action::GoalResponse::REJECT;
    	}

    	return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse 
    AgvPeripheral::cancelCB(
	    const std::shared_ptr<GoalHandle> goal_handle) {
	    DEBUG_STREAM_OUT("Received cancel request");
	    (void)goal_handle;
	    return rclcpp_action::CancelResponse::ACCEPT;
  	}

    void AgvPeripheral::acceptedCB(const std::shared_ptr<GoalHandle> goal_handle)
    {
    	if (current_goal_handle_) {
	        auto result = std::make_shared<PeripheralAction::Result>();
	        result->success = FAILED;
	        result->message = "Another action is in progress";
	        goal_handle->abort(result);
	        return;
	    }
	    
    	std::thread{ 
		        [this, goal_handle]() { execute_action(goal_handle); }
		    }.detach();
    }

    void AgvPeripheral::execute_action(const std::shared_ptr<GoalHandle> goal_handle) 
    {
    	{
	        std::lock_guard<std::mutex> lock(data_mutex_);
	        current_goal_handle_ = goal_handle;
	        current_result_ = std::make_shared<PeripheralAction::Result>();
	        current_feedback_ = std::make_shared<PeripheralAction::Feedback>();
    	}

    	const auto goal = goal_handle->get_goal();
    	current_action = goal->goal_action;
		DEBUG_STREAM_OUT("goal action:" 
			<< "type=" << current_action.actiontype 
			<< ", id=" << current_action.actionid 
			<< ", description=" << current_action.actiondescription 
			<< ", blocking=" << current_action.blockingtype
			<< ", parameters_count=" << current_action.actionparameters.size());
		for (const auto& param : current_action.actionparameters) {
			DEBUG_STREAM_OUT(
				"  Parameter: "
				<< "name=" << param.key 
				<< ", value=" << param.value
				<< ", type=" << param.valuedatatype
			);
		}

		int action_type = 0;
		// int action_id = atoi(current_action.actionId.c_str());
		if (current_action.actiontype.compare(action_pick) == 0 || current_action.actiontype.compare(ORDER_UP_OLD_VERSION) == 0)
		{
			action_type = UP;
		}
		else if (current_action.actiontype.compare(action_drop) == 0 || current_action.actiontype.compare(ORDER_DOWN_OLD_VERSION) == 0)
		{
			action_type = DOWN;
		}
		else if (current_action.actiontype.compare(action_adjust) == 0)
		{
			action_type = ROTATION;
			rotary_target_pos = 0.0;
			for (int i = 0; i < (int)(current_action.actionparameters.size()); i++)
			{
				if (current_action.actionparameters[i].key.compare(string("angle")) == 0)
				{
					bool fail = false;
					double angle = 0.0;
					if (current_action.actionparameters[i].value.length() == 0)
					{
						DEBUG_OUT("goalCB:  bad value length");
						fail = true;
					}

					try
					{
						angle = std::stof(current_action.actionparameters[i].value);
					}
					catch (...)
					{
						fail = true;
						DEBUG_ERROR_OUT("error: string2float failed");
					}

					if (fail)
					{
						current_result_->success = FAILED;
						current_result_->message = "Operate fail: bad target";
						goal_handle->abort(current_result_);
						clear_current_action();
						return;
					}

					rotary_target_pos = angle / 180.0 * M_PI * -1;
				}
			}
		}
		else
		{
			action_type = NO_OP;
		}

		if (get_cur_state() != IDLE_STATE)
		{
			current_result_->success = FAILED;
			current_result_->message = "ERROR Device is busy";
			current_result_->result_action = current_action;
			goal_handle->abort(current_result_);
			clear_current_action();
			// memset(&current_action, 0, sizeof(current_action));
			DEBUG_WARN_OUT("[AgvPeripheral] last task is not completed");
			return;
		}

		switch (action_type)
		{
		case NO_OP:
		case UP:
		case DOWN:
		case CLOCKWISE_ROTATION:
		case ANTICLOCKWISE_ROTATION:
		case ROTATION:
			set_cur_op((periph_op_enum)action_type);
			set_next_op(NO_OP);
			break;

		case UP_CLOCKWISE_ROTATION:
			set_cur_op(UP);
			set_next_op(CLOCKWISE_ROTATION);
			break;

		case UP_ANTICLOCKWISE_ROTATION:
			set_cur_op(UP);
			set_next_op(ANTICLOCKWISE_ROTATION);
			break;

		case CLOCKWISE_ROTATION_DOWN:
			set_cur_op(CLOCKWISE_ROTATION);
			set_next_op(DOWN);
			break;

		case ANTICLOCKWISE_ROTATION_DOWN:
			set_cur_op(ANTICLOCKWISE_ROTATION);
			set_next_op(DOWN);
			break;

		default:
			break;
		}

		if (!executeOP(action_type)) {
			current_result_->success = FAILED;
			current_result_->message = "Operation not defined";
			current_result_->result_action = current_action;
			goal_handle->abort(current_result_);
			clear_current_action();
			// memset(&current_action, 0, sizeof(current_action));
			set_cur_state(IDLE_STATE);
			DEBUG_WARN_OUT("[AgvPeripheral] Operate failed");
			return;
		}
	}

	void AgvPeripheral::handle_preemption()
	{
		DEBUG_OUT("[AgvPeripheral] %s: Preempted", name.c_str());
		current_result_->result_action = current_action;

		if (current_goal_handle_->is_active())
		{
			if ((get_cur_op() == UP) || (get_cur_op() == DOWN))
			{
				pallet_controller->set_jacking_target_position(joint_pos[0]);
				DEBUG_OUT("[AgvPeripheral] cancel the jacking joint goal");
			}
			else if ((get_cur_op() == CLOCKWISE_ROTATION) || (get_cur_op() == ANTICLOCKWISE_ROTATION) || (get_cur_op() == ROTATION))
			{
				pallet_controller->set_rotary_target_position(joint_pos[1]);
				DEBUG_OUT("[AgvPeripheral] cancel the rotary joint goal");
			}
			clear_os();
		}
		// set the action state to preempted
		current_result_->success = CANCELED;
		current_result_->message = "Action has been preempted";
		current_result_->result_action = current_action;
		current_goal_handle_->canceled(current_result_);
		clear_current_action();
		// memset(&current_action, 0, sizeof(current_action));
	}

	bool AgvPeripheral::periphRecordPosRead()
	{
		std::fstream fin(pos_record_file.c_str(), std::ios::in);
		if (fin.fail())
		{
			DEBUG_ERROR_OUT("could not open %s.", pos_record_file.c_str());
			return false;
		}

		try
		{
			YAML::Node yamlConfig = YAML::Load(fin);
			fin.close();
			if (!yamlConfig)
			{
				DEBUG_ERROR_OUT("read %s faild. data is not yaml format", pos_record_file.c_str());
				return false;
			}
			if (yamlConfig["jacking_joint_pos"] && yamlConfig["rotary_joint_pos"])
			{
				file_record_pos[0] = yamlConfig["jacking_joint_pos"].as<float>();
				file_record_pos[1] = yamlConfig["rotary_joint_pos"].as<float>();
				if (file_record_pos[0] > max_pos[0])
					file_record_pos[0] = max_pos[0];
				if (file_record_pos[0] < 0.00)
					file_record_pos[0] = 0.00;
				if (file_record_pos[1] > M_PI)
					file_record_pos[1] = M_PI;
				if (file_record_pos[1] < -1 * M_PI)
					file_record_pos[1] = -1 * M_PI;
				DEBUG_OUT("read pos success, jacking joint pos is %.4f rotary joint pos is %.4f", file_record_pos[0], file_record_pos[1]);
			}
			else
			{
				DEBUG_ERROR_OUT("read %s faild. data is not exist", pos_record_file.c_str());
				return false;
			}
		}
		catch (const YAML::Exception &e)
		{
			DEBUG_ERROR_OUT("file format error %s,%s.", pos_record_file.c_str(), e.what());
			return false;
		}
		return true;
	}

	bool AgvPeripheral::periphRecordPosWrite()
	{
		// TODO: shouldn't open/close every seconds.
		// 如果数据不变化，则不持续写入
		if (abs(last_joint_record_pos[0] - joint_pos[0]) < 0.002 && abs(last_joint_record_pos[1] - joint_pos[1]) < 0.002)
		{
			return true;
		}

		std::fstream fout(pos_record_file.c_str(), std::ios::out);
		if (fout.fail())
		{
			DEBUG_ERROR_OUT("could not creat file %s.", pos_record_file.c_str());
			return false;
		}

		YAML::Node yamlConfig;
		yamlConfig["jacking_joint_pos"] = joint_pos[0];
		yamlConfig["rotary_joint_pos"] = normalizationRad(joint_pos[1]);
		// DEBUG_OUT("write jacking joint pos %.4f rotary joint pos %.4f", joint_pos[0], joint_pos[1]);
		fout << yamlConfig;
		fout.flush();
		fout.close();

		last_joint_record_pos[0] = joint_pos[0];
		last_joint_record_pos[1] = joint_pos[1];

		return true;
	}

	void AgvPeripheral::periphPosRecord()
	{
		if (IDLE_STATE == get_cur_state() || MANUAL_MODE_STATE == get_cur_state())
		{
			if (!periphRecordPosWrite())
			{
				DEBUG_ERROR_OUT("write record pos fail");
			}
		}
	}

	void AgvPeripheral::thread_cycle()
	{
		periphPosRecord();
		check_jointState_out();
	}

	void AgvPeripheral::thread_fun(AgvPeripheral *pThis)
	{
		if (!pThis)
			return;

		while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(1000)))
		{
			pThis->thread_cycle();
		}
	}

	int AgvPeripheral::callControllerService(const std::vector<std::string>& controllers, const bool enable)
	{
		if (!PeripheralEnableCtl->service_is_ready()) {
			DEBUG_ERROR_OUT("Controller service client is not ready");
			return -1;
		}
		auto request = std::make_shared<agv_srvs::srv::ControllerEnable::Request>();
		request->controller_names = controllers;
		request->enable = enable;

		auto future_result = PeripheralEnableCtl->async_send_request(request);

		if (future_result.wait_for(std::chrono::seconds(3)) == std::future_status::timeout) {
		    DEBUG_ERROR_OUT("Service call to %s timed out", 
		                PeripheralEnableCtl->get_service_name());
		    return -1;
		}

		auto response = future_result.get();
		return response->success;
	}

	bool AgvPeripheral::get_shared_parameters()
	{
		if (!SharedParamsInterface::initialize()) {
			DEBUG_ERROR_OUT("Failed to initialize shared memory interface");
			return false;
		}

		try {

			auto params = SharedParamsInterface::getNodeParams("shared_parameters");
			if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
			  DEBUG_ERROR_OUT("Error get shared parameters");
			  return false;
			} 

			params = SharedParamsInterface::getNodeParams("agv_peripheral_controller");
			for (const auto& [name, value] : params) {
			  	DEBUG_STREAM_OUT("param:" << name << ", value:" << value.toString());
			     switch(value.type) {
			        case SharedParamsInterface::ParamValue::Type::INT:
			            this->declare_parameter(name, value.getValue<int>());
			            break;
			        case SharedParamsInterface::ParamValue::Type::DOUBLE:
			            this->declare_parameter(name, value.getValue<double>());
			            break;
			        case SharedParamsInterface::ParamValue::Type::STRING:
			            this->declare_parameter(name, value.getValue<std::string>());
			            break;
			        case SharedParamsInterface::ParamValue::Type::BOOL:
			            this->declare_parameter(name, value.getValue<bool>());
			            break;
			        default:
			           	DEBUG_WARN_OUT("Unsupported parameter type for %s", name.c_str());
			            break;
			    }
			}

			params = SharedParamsInterface::getNodeParams("IO");
			for (const auto& [name, value] : params) {
				if (name.find("output_device/liftRelay") != std::string::npos) {
					DEBUG_STREAM_OUT("Find liftRelay:" << name << ", value:" << value.toString());
					hasliftrelay = true;
				}
			}
		} catch (const std::exception& e) {
			DEBUG_ERROR_OUT("Error reading parameters: %s ", e.what());
			return false;
		}
		return true;
	}

}; // end namespace

/** @brief main function */
int main(int argc, char **argv)
{
	// Initialize the ROS node
	rclcpp::init(argc, argv);
	std::shared_ptr<std::thread> worker_thread = nullptr; 

	try {
	    auto node = std::make_shared<agv_peripheral::AgvPeripheral>("AgvPeripheral");
	    if(!node->init()) {
	        DEBUG_OUT("Node initialization failed");
	        return 1;
	    }

	    worker_thread = std::make_shared<std::thread>(
	        [node]() { node->spin(); }
	    );

	    rclcpp::executors::MultiThreadedExecutor executor;
	    executor.add_node(node);
	    executor.spin();

	} catch (const std::exception& e) {
	    DEBUG_OUT("Exception in main: %s", e.what());
	  
	    if (worker_thread && worker_thread->joinable()) {
	        worker_thread->join();
	    }
	    
	    rclcpp::shutdown();
	    return 2;
	}

	if (worker_thread && worker_thread->joinable()) {
	    worker_thread->join();
	}

	rclcpp::shutdown();
	return 0;
}
